// pid.cpp
#include "pid.h"

PID::PID(double kp, double ki, double kd, double out_min, double out_max)
    : Kp(kp), Ki(ki), Kd(kd),
      prev_error(0.0), integral(0.0),
      out_min(out_min), out_max(out_max) {}

void PID::reset() {
    prev_error = 0.0;
    integral = 0.0;
}

double PID::compute(double setpoint, double measurement, double dt) {
    double error = setpoint - measurement;
    integral += error * dt;
    double derivative = (error - prev_error) / dt;
    double output = Kp * error + Ki * integral + Kd * derivative;

    if (output > out_max) output = out_max;
    if (output < out_min) output = out_min;

    prev_error = error;
    return output;
}
